#include "Motor.h"
#include "driver_gpio.h"
#include "driver_pwm.h"
#include "sys_utils.h"

void motor_init()
{
    GPIO_InitTypeDef MOTOR_GPIO_Handle;
    struct_PWM_Config_t MOTOR_Step_Config;
    __SYSTEM_GPIO_CLK_ENABLE();
    __SYSTEM_PWM_CLK_ENABLE(); 
    
    MOTOR_GPIO_Handle.Pin = Motor_VPP_C_Pin;
    MOTOR_GPIO_Handle.Mode = GPIO_MODE_OUTPUT_PP;
    MOTOR_GPIO_Handle.Pull = GPIO_PULLUP;
    gpio_init(Motor_VPP_C_Port, &MOTOR_GPIO_Handle);
    
    MOTOR_GPIO_Handle.Pin = Motor_Sleep_Pin;
    MOTOR_GPIO_Handle.Mode = GPIO_MODE_OUTPUT_PP;
    MOTOR_GPIO_Handle.Pull = GPIO_NOPULL;
    gpio_init(Motor_Sleep_Port, &MOTOR_GPIO_Handle);

    MOTOR_GPIO_Handle.Pin = Motor_Dir_Pin;
    MOTOR_GPIO_Handle.Mode = GPIO_MODE_OUTPUT_PP;
    MOTOR_GPIO_Handle.Pull = GPIO_PULLDOWN;
    gpio_init(Motor_Dir_Port, &MOTOR_GPIO_Handle);

    MOTOR_GPIO_Handle.Pin = Motor_Enable_Pin;
    MOTOR_GPIO_Handle.Mode = GPIO_MODE_OUTPUT_PP;
    MOTOR_GPIO_Handle.Pull = GPIO_NOPULL;
    gpio_init(Motor_Enable_Port, &MOTOR_GPIO_Handle);

    MOTOR_GPIO_Handle.Pin = Motor_Step_Pin;
    MOTOR_GPIO_Handle.Mode = GPIO_MODE_AF_PP;
    MOTOR_GPIO_Handle.Pull = GPIO_PULLUP;
    MOTOR_GPIO_Handle.Alternate = GPIO_FUNCTION_6;
    gpio_init(Motor_Step_Port, &MOTOR_GPIO_Handle);

    MOTOR_Step_Config.Prescale = 48;
    MOTOR_Step_Config.Period   = 1300;
    MOTOR_Step_Config.Posedge  = 0;
    MOTOR_Step_Config.Negedge  = 650;
    pwm_config(PWM_CHANNEL_0|PWM_CHANNEL_1|PWM_CHANNEL_2, MOTOR_Step_Config);

}
void motor_power_on()
{
    gpio_write_pin(Motor_VPP_C_Port,Motor_VPP_C_Pin,GPIO_PIN_CLEAR);
}
void motor_power_off()
{
    gpio_write_pin(Motor_VPP_C_Port,Motor_VPP_C_Pin,GPIO_PIN_SET);
}
void motor_forward()
{
    motor_power_on();
    gpio_write_pin(Motor_Sleep_Port,Motor_Sleep_Pin,GPIO_PIN_SET);
    co_delay_100us(50);
    gpio_write_pin(Motor_Enable_Port ,Motor_Enable_Pin,GPIO_PIN_CLEAR);
    gpio_write_pin(Motor_Dir_Port,Motor_Dir_Pin,GPIO_PIN_CLEAR);
    pwm_output_enable(PWM_CHANNEL_0|PWM_CHANNEL_1|PWM_CHANNEL_2);  

}
void motor_reverse()
{
    motor_power_on();
    gpio_write_pin(Motor_Sleep_Port,Motor_Sleep_Pin,GPIO_PIN_SET);
    co_delay_100us(50);
    gpio_write_pin(Motor_Enable_Port ,Motor_Enable_Pin,GPIO_PIN_CLEAR);
    gpio_write_pin(Motor_Dir_Port,Motor_Dir_Pin,GPIO_PIN_SET);
    pwm_output_enable(PWM_CHANNEL_0);
}
void motor_stop()
{
    pwm_output_disable(PWM_CHANNEL_0);
    gpio_write_pin(Motor_Enable_Port ,Motor_Enable_Pin,GPIO_PIN_SET);
    gpio_write_pin(Motor_Sleep_Port,Motor_Sleep_Pin,GPIO_PIN_CLEAR);
    motor_power_off();
}


